<?xml version="1.0" encoding="utf-8"?>
<sdf version='1.6'>

  {# import templates from the MRS UAV System #}
  {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as mrs_components -%}
  {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as mrs_generic -%}

  {# import additional components from this package, path is relative to package root #}
  {%- import 'models/my_drone/sdf/custom_macros.sdf.jinja' as custom_macros -%}

  {# Robot params {--> #}
  {%- set mass = 2.0 -%} {# [kg] #}
  {%- set body_radius = 0.1 -%} {# [m] #}
  {%- set body_height = 0.017 -%} {# [m] #}

  {%- set prop_mass = 0.01 -%} {# [kg] #}
  {%- set prop_radius  = 0.09 -%} {# [m] #}
  {%- set prop_offset_z = -0.004 -%} {# [m] #}

  {%- set center_board_radius = 0.05 -%} {# [m] #}
  {%- set center_board_height= 0.002 -%} {# [m] #}
  {%- set center_board_offset_z = 0.01 -%} {# [m] #}
  {%- set pixhawk_offset_z = -0.004 -%} {# [m] #}

  {%- set arm_length = 0.166 -%} {# [m] #}
  {%- set arm_offset_z = 0.01 -%} {# [m] #}

  {%- set leg_height = 0.065 -%} {# [m] #}
  {%- set leg_radius = 0.012 -%} {# [m] #}
  {%- set leg_offset_radial = -0.01 -%} {# [m] #}
  {%- set leg_offset_z = -0.033 -%} {# [m] #}

  {%- set motor_radius = 0.015 -%} {# [m] #}
  {%- set motor_height = 0.013 -%} {# [m] #}
  {%- set motor_offset_z = -0.01 -%} {# [m] #}

  {%- set root = 'base_link' -%}
  {# <!--}--> #}

  {# Motor constants {--> #}
  {%- set rotor_velocity_slowdown_sim = 0.0159236 -%}
  {%- set motor_constant = 8.91 -%} {# [kg.m/s^2] #}
  {%- set moment_constant = 0.016 -%} {# [m] #}
  {%- set time_constant_up = 1.0 / 80.0 -%} {# [s] #}
  {%- set time_constant_down = 1.0 / 40.0 -%} {# [s] #}
  {%- set max_rot_velocity = 1 -%} {# [rad/s] #}
  {%- set rotor_drag_coefficient = 0.001 -%} {# orig 8.06428e-04 #}
  {%- set rolling_moment_coefficient = '1.0e-6' -%}
  {# <!--}--> #}

  {# Meshes {--> #}
  {%- set arm_mesh_file = 'model://my_drone/meshes/arm.dae' -%} {# from this package #}
  {%- set prop_mesh_file = 'model://my_drone/meshes/propeller.dae' -%} {# from this package #}
  {%- set pixhawk_mesh_file = 'model://mrs_robots_description/meshes/sensors/pixhawk.dae' -%} {# from mrs_uav_gazebo_simulation #}
  {%- set mesh_scale = '1 1 1' -%}
  {%- set prop_mesh_scale_ccw = '0.8 0.8 1' -%}
  {%- set prop_mesh_scale_cw = '-0.8 0.8 1' -%}
  {# <!--}--> #}

  {# Inertias {--> #}
  {%- set inertia_body_radius = 0.2 -%} {# [m] #}
  {%- set inertia_body_height = 0.075 -%} {# [m] #}

  {%- set body_ixx = mass * (3 * inertia_body_radius * inertia_body_radius + inertia_body_height * inertia_body_height) / 12 -%}
  {%- set body_ixy = 0 -%}
  {%- set body_ixz = 0 -%}
  {%- set body_iyy = mass * (3 * inertia_body_radius * inertia_body_radius + inertia_body_height * inertia_body_height) / 12 -%}
  {%- set body_iyz = 0 -%}
  {%- set body_izz = (mass * inertia_body_radius * inertia_body_radius) / 2 -%}

  {%- set prop_ixx = 0.0001 -%}
  {%- set prop_ixy = 0 -%}
  {%- set prop_ixz = 0 -%}
  {%- set prop_iyy = 0.0001 -%}
  {%- set prop_iyz = 0 -%}
  {%- set prop_izz = 0.0001 -%}
  {# <!--}--> #}

  <model name="{{ spawner_args['name'] }}">

    <link name="{{ root }}">

      <!-- Body physics {-->
      {{ mrs_generic.multirotor_physics_macro(
        mass = mass,
        body_radius = body_radius,
        body_height = body_height,
        rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim,
        ixx = body_ixx,
        ixy = body_ixy,
        ixz = body_ixz,
        iyy = body_iyy,
        iyz = body_iyz,
        izz = body_izz)
      }}
      <!--}-->

      <!-- Body visuals {-->

      <!-- Center boards {-->
      {{ custom_macros.visual_cylinder_macro(
        name = 'center_board_top',
        height = center_board_height,
        radius = center_board_radius,
        color = 'DarkGrey',
        x = 0,
        y = 0,
        z = center_board_offset_z,
        roll = 0,
        pitch = 0,
        yaw = 0)
      }}
      {{ custom_macros.visual_cylinder_macro(
        name = 'center_board_bottom',
        height = center_board_height,
        radius = center_board_radius,
        color = 'DarkGrey',
        x = 0,
        y = 0,
        z = -center_board_offset_z - arm_offset_z - body_height / 2,
        roll = 0,
        pitch = 0,
        yaw = 0)
      }}
      <!--}-->

      <!-- Arms {-->
      {{ mrs_generic.visual_mesh_macro(
        name = 'front_right_arm',
        mesh_file = arm_mesh_file,
        mesh_scale = mesh_scale,
        color = 'DarkGrey',
        x = center_board_radius * math.cos(math.radians(-37)),
        y = center_board_radius * math.sin(math.radians(-37)),
        z = arm_offset_z,
        roll = 0,
        pitch = 0,
        yaw = math.radians(-37))
      }}

      {{ mrs_generic.visual_mesh_macro(
        name = 'front_left_arm',
        mesh_file = arm_mesh_file,
        mesh_scale = mesh_scale,
        color = 'DarkGrey',
        x = center_board_radius * math.cos(math.radians(37)),
        y = center_board_radius * math.sin(math.radians(37)),
        z = arm_offset_z,
        roll = 0,
        pitch = 0,
        yaw = math.radians(37))
      }}

      {{ mrs_generic.visual_mesh_macro(
        name = 'back_left_arm',
        mesh_file = arm_mesh_file,
        mesh_scale = mesh_scale,
        color = 'Indigo',
        x = center_board_radius * math.cos(math.radians(143)),
        y = center_board_radius * math.sin(math.radians(143)),
        z = arm_offset_z,
        roll = 0,
        pitch = 0,
        yaw = math.radians(143))
      }}

      {{ mrs_generic.visual_mesh_macro(
        name = 'back_right_arm',
        mesh_file = arm_mesh_file,
        mesh_scale = mesh_scale,
        color = 'Indigo',
        x = center_board_radius * math.cos(math.radians(217)),
        y = center_board_radius * math.sin(math.radians(217)),
        z = arm_offset_z,
        roll = 0,
        pitch = 0,
        yaw = math.radians(217))
      }}
      <!--}-->

      <!-- Pixhawk {-->
      {{ mrs_generic.visual_mesh_macro(
        name = 'pixhawk',
        mesh_file = pixhawk_mesh_file,
        mesh_scale = mesh_scale,
        color = 'DarkGrey',
        x = 0,
        y = 0,
        z = body_height + pixhawk_offset_z,
        roll = 0,
        pitch = 0,
        yaw = 0)
      }}
      <!--}-->

      <!-- Legs {-->
      {{ custom_macros.visual_cylinder_with_collision_macro(
        name = 'front_right_leg',
        height = leg_height,
        radius = leg_radius,
        color = 'DarkGrey',
        x = (center_board_radius + arm_length + leg_offset_radial) * math.cos(math.radians(-37)),
        y = (center_board_radius + arm_length + leg_offset_radial) * math.sin(math.radians(-37)),
        z = leg_offset_z,
        roll = 0,
        pitch = 0,
        yaw = 0)
      }}
      {{ custom_macros.visual_cylinder_with_collision_macro(
        name = 'front_left_leg',
        height = leg_height,
        radius = leg_radius,
        color = 'DarkGrey',
        x = (center_board_radius + arm_length + leg_offset_radial) * math.cos(math.radians(37)),
        y = (center_board_radius + arm_length + leg_offset_radial) * math.sin(math.radians(37)),
        z = leg_offset_z,
        roll = 0,
        pitch = 0,
        yaw = 0)
      }}
      {{ custom_macros.visual_cylinder_with_collision_macro(
        name = 'back_left_leg',
        height = leg_height,
        radius = leg_radius,
        color = 'Indigo',
        x = (center_board_radius + arm_length + leg_offset_radial) * math.cos(math.radians(143)),
        y = (center_board_radius + arm_length + leg_offset_radial) * math.sin(math.radians(143)),
        z = leg_offset_z,
        roll = 0,
        pitch = 0,
        yaw = 0)
      }}
      {{ custom_macros.visual_cylinder_with_collision_macro(
        name = 'back_right_leg',
        height = leg_height,
        radius = leg_radius,
        color = 'Indigo',
        x = (center_board_radius + arm_length + leg_offset_radial) * math.cos(math.radians(217)),
        y = (center_board_radius + arm_length + leg_offset_radial) * math.sin(math.radians(217)),
        z = leg_offset_z,
        roll = 0,
        pitch = 0,
        yaw = 0)
      }}
      <!--}-->

      <!-- Motors {-->
      {{ custom_macros.visual_cylinder_macro(
        name = 'front_right_motor',
        height = motor_height,
        radius = motor_radius,
        color = 'DarkGrey',
        x = (center_board_radius + arm_length) * math.cos(math.radians(-37)),
        y = (center_board_radius + arm_length) * math.sin(math.radians(-37)),
        z = body_height + arm_offset_z + motor_offset_z,
        roll = 0,
        pitch = 0,
        yaw = 0)
      }}
      {{ custom_macros.visual_cylinder_macro(
        name = 'front_left_motor',
        height = motor_height,
        radius = motor_radius,
        color = 'DarkGrey',
        x = (center_board_radius + arm_length) * math.cos(math.radians(37)),
        y = (center_board_radius + arm_length) * math.sin(math.radians(37)),
        z = body_height + arm_offset_z + motor_offset_z,
        roll = 0,
        pitch = 0,
        yaw = 0)
      }}
      {{ custom_macros.visual_cylinder_macro(
        name = 'back_left_motor',
        height = motor_height,
        radius = motor_radius,
        color = 'DarkGrey',
        x = (center_board_radius + arm_length) * math.cos(math.radians(143)),
        y = (center_board_radius + arm_length) * math.sin(math.radians(143)),
        z = body_height + arm_offset_z + motor_offset_z,
        roll = 0,
        pitch = 0,
        yaw = 0)
      }}
      {{ custom_macros.visual_cylinder_macro(
        name = 'back_right_motor',
        height = motor_height,
        radius = motor_radius,
        color = 'DarkGrey',
        x = (center_board_radius + arm_length) * math.cos(math.radians(217)),
        y = (center_board_radius + arm_length) * math.sin(math.radians(217)),
        z = body_height + arm_offset_z + motor_offset_z,
        roll = 0,
        pitch = 0,
        yaw = 0)
      }}
      <!--}-->

      <!--}-->

    </link>

    {# Propellers {--> #}
    {%- set prop_list = [
    {
    'motor_number': 0,
    'direction': 'cw',
    'x': (center_board_radius + arm_length) * math.cos(math.radians(-37)),
    'y': (center_board_radius + arm_length) * math.sin(math.radians(-37)),
    'z': body_height + arm_offset_z + motor_height + motor_offset_z + prop_offset_z,
    'mesh_files': [prop_mesh_file],
    'mesh_scale': prop_mesh_scale_cw,
    'color': 'Gold'
    },
    {
    'motor_number': 1,
    'direction': 'cw',
    'x': (center_board_radius + arm_length) * math.cos(math.radians(143)),
    'y': (center_board_radius + arm_length) * math.sin(math.radians(143)),
    'z': body_height + arm_offset_z + motor_height + motor_offset_z + prop_offset_z,
    'mesh_files': [prop_mesh_file],
    'mesh_scale': prop_mesh_scale_cw,
    'color': 'Gold'
    },
    {
    'motor_number': 2,
    'direction': 'ccw',
    'x': (center_board_radius + arm_length) * math.cos(math.radians(37)),
    'y': (center_board_radius + arm_length) * math.sin(math.radians(37)),
    'z': body_height + arm_offset_z + motor_height + motor_offset_z + prop_offset_z,
    'mesh_files': [prop_mesh_file],
    'mesh_scale': prop_mesh_scale_ccw,
    'color': 'Gold'
    },
    {
    'motor_number': 3,
    'direction': 'ccw',
    'x': (center_board_radius + arm_length) * math.cos(math.radians(217)),
    'y': (center_board_radius + arm_length) * math.sin(math.radians(217)),
    'z': body_height + arm_offset_z + motor_height + motor_offset_z + prop_offset_z,
    'mesh_files': [prop_mesh_file],
    'mesh_scale': prop_mesh_scale_ccw,
    'color': 'Gold'
    }
    ]
    -%}
    {{ mrs_components.propellers_macro(
      prop_list = prop_list,
      rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim,
      motor_constant = motor_constant,
      moment_constant = moment_constant,
      parent = root,
      mass = prop_mass,
      radius = prop_radius,
      time_constant_up = time_constant_up,
      time_constant_down = time_constant_down,
      max_rot_velocity = max_rot_velocity,
      rotor_drag_coefficient = rotor_drag_coefficient,
      rolling_moment_coefficient = rolling_moment_coefficient,
      meshes_z_offset = 0,
      prop_ixx = prop_ixx,
      prop_ixy = prop_ixy,
      prop_ixz = prop_ixz,
      prop_iyy = prop_iyy,
      prop_iyz = prop_iyz,
      prop_izz = prop_izz,
      spawner_args = spawner_args)
    }}
    {# <!--}--> #}

    <!-- Pixhawk {-->
    {%- set imu_topic = '/imu' -%}
    {%- set mag_topic = '/mag' -%}
    {%- set baro_topic = '/baro' -%}
    {%- set lidar_topic = '/lidar' -%}

    <!-- Gazebo ground truth {-->
    {{ mrs_generic.gazebo_groundtruth_macro(
      home_latitude = 0,
      home_longitude = 0,
      home_altitude = 0)
    }}
    <!--}-->

    <!-- GPS {-->
    {{ mrs_generic.gazebo_gps_macro(
      gps_name = 'gps0',
      parent_link = root,
      update_rate = 10,
      gps_noise = true,
      gps_xy_random_walk = 2.0,
      gps_z_random_walk = 4.0,
      gps_xy_noise_density = '2.0e-4',
      gps_z_noise_density = '4.0e-4',
      gps_vxy_noise_density = 0.2,
      gps_vz_noise_density = 0.4,
      x = 0,
      y = 0,
      z = 0,
      roll = 0,
      pitch = 0,
      yaw = 0)
    }}
    <!--}-->

    <!-- Magnetometer {-->
    {{ mrs_generic.gazebo_magnetometer_macro(
      pub_rate = 100,
      noise_density = 0.0004,
      random_walk = 0.0000064,
      bias_correlation_time = 600,
      mag_topic = mag_topic)
    }}
    <!--}-->

    <!-- Barometer {-->
    {{ mrs_generic.gazebo_barometer_macro(
      baro_topic = baro_topic,
      pub_rate = 50,
      baro_drift_pa_per_sec = 0)
    }}
    <!--}-->

    <!-- Mavlink interface (customized) {-->
    {{ mrs_generic.gazebo_mavlink_interface_macro(
      imu_topic = imu_topic,
      mag_topic = mag_topic,
      baro_topic = baro_topic,
      lidar_topic = lidar_topic,
      mavlink_config = spawner_args['mavlink_config'])
    }}
    <!--}-->

    <!-- IMU {-->
    <!-- NOTE: IMU has to be last, otherwise the simulation is extremely slow! -->
    {{ mrs_generic.gazebo_imu_macro(
      imu_name = 'imu',
      parent_link = root,
      imu_topic = imu_topic,
      gyroscope_noise_density = 0.00018665,
      gyroscope_random_walk = 0.000038785,
      gyroscope_bias_correlation_time = 1000.0,
      gyroscope_turn_on_bias_sigma = 0.0087,
      accelerometer_noise_density = 0.00186,
      accelerometer_random_walk = 0.006,
      accelerometer_bias_correlation_time = 300.0,
      accelerometer_turn_on_bias_sigma = 0.1960,
      x = 0,
      y = 0,
      z = 0,
      roll = 0,
      pitch = 0,
      yaw = 0)
    }}
    <!--}-->

    <!--}-->

    {# ================================================================== #}
    {# ||                  optional sensor definitions                 || #}
    {# ================================================================== #}

    {# Ground truth {--> #}
    {{ mrs_components.ground_truth_macro(
      parent_link = root,
      x = 0,
      y = 0,
      z = 0,
      roll = 0,
      pitch = 0,
      yaw = 0,
      spawner_args = spawner_args)
    }}
    {# <!--}--> #}

    {# Garmin rangefinder (laser altimeter) {--> #}
    {%- set garmin_rangefinder = mrs_components.garmin_down_macro(
      parent_link = root,
      x = 0.13,
      y = 0,
      z = -0.005,
      roll = 0,
      pitch = math.radians(90),
      yaw = 0,
      mount = none,
      spawner_args = spawner_args)
    -%}
    {{ garmin_rangefinder }}
    {# <!--}--> #}

    {# Custom monochrome camera {--> #}
    {%- set monochrome_camera = custom_macros.custom_monochrome_camera_macro(
      camera_name = 'my_camera',
      parent_link = root,
      x = 0.13,
      y = 0,
      z = 0.01,
      roll = 0,
      pitch = 0,
      yaw = 0,
      spawner_args = spawner_args)
    -%}
    {{ monochrome_camera }}
    {# <!--}--> #}

    {# Optional sensor mount {--> #}
    {%- set optional_sensor_mount -%}
    <link name="optional_sensor_mount_link">
      {{ mrs_generic.zero_inertial_macro() }}
      <pose>0.13 0 0 0.01 0 0</pose>
      <visual name="optional_sensor_mount_visual">
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <box>
            <size>0.03 0.2 0.004</size>
          </box>
        </geometry>
        <material>
          <script>
            <name>Gazebo/DarkGrey</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
    </link>
    <joint name="optional_sensor_mount_joint" type="fixed">
      <child>optional_sensor_mount_link</child>
      <parent>{{ root }}</parent>
    </joint>
    {%- endset -%}
    {# <!--}--> #}

    {%- if garmin_rangefinder | length > 0 or monochrome_camera | length > 0 -%}
      <!-- optional sensor mount {-->
      {{ optional_sensor_mount }}
      <!--}-->
    {%- endif -%}

  </model>
</sdf>

